#include "turret.h"
#include "../Commands/TurretJoy.h"
#include "../CommandBase.h"
#define I_CONST 18
#define P_CONST 1320
#define D_CONST 10
// put that somewhere else ^^^

Turret::Turret() : Subsystem("turret")
{	
	turretJaguar = new CANJaguar(TURRET_JAGUAR, CANJaguar::kPosition);
	turretJaguar->SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
	turretJaguar->ConfigEncoderCodesPerRev(2048);
	turretJaguar->SetVoltageRampRate(50);
	turretJaguar->EnableControl(0);
	desiredPosition = 0;
}

void Turret::turretJoystick(Joystick *joystick)
{
	turretJaguar->Set(desiredPosition);
	desiredPosition = joystick->GetY();
	turretJaguar->SetPID(P_CONST,I_CONST,D_CONST);
}

void Turret::InitDefaultCommand()
{
	SetDefaultCommand(new TurretJoy());
}
